/*
S.Shanmugaesan (1061630)  Michael Antemann (1061753)
Computergrafik II
30.04.09
*/

#include <GL/gl.h>
#include <GL/glut.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#define x01 -100
#define y01  0

#define x04 100
#define y04 0

#define x1a2 0
#define y1a2 60

#define x3a2a -240
#define y3a2a -100

#define x4a3 40
#define y4a3 160

#define genauigkeit 0.000000001
#define iterationsAnzahl 100

// Koordinaten für den Körper2:
double x_koerper1 = -100;
double y_koerper1 = 60;

double x_koerper3 = 140;
double y_koerper3 = 160;

//winkel
int theta1_deg = 0;	//eingabewinkel als DEg
double theta1_rad;	//der Eingabewinkel (als Rad)
double theta2_rad;
double theta3_rad;
double theta4_rad;	//Der Errechnete Winkel

//delta
double delta = 0.000001;

//Umwandeln von degres in rad
double DegToRad(int d);
double RadToDeg(double r);

// Funktion zur Berechnung der passenden Koordinaten für den Körper2:
double berechneKoordinatenFuerKoerper2()
{
      // Berechnung der Koordinaten für den Körper 2:
             x_koerper1 = 0.0 * cos(theta1_rad) - 60.0 * sin(theta1_rad) - 100.0;
             y_koerper1 = 60.0 * cos(theta1_rad) + 0.0 * sin(theta1_rad);
             x_koerper3 = 40.0 * cos(theta4_rad) - 160.0 * sin(theta4_rad) + 100.0;
             y_koerper3 = 160.0 *cos(theta4_rad) + 40.0 * sin(theta4_rad);

}
//erste Gleichung
double ersteGleichung(double theta1_rad,double theta3_rad, double theta4_rad)
{
	return(x01-x04-x4a3*cos(theta4_rad)-y1a2*sin(theta1_rad)+
			y4a3*sin(theta4_rad)+y3a2a*cos(theta4_rad)*sin(theta3_rad)+
			y3a2a*cos(theta3_rad)*sin(theta4_rad)-x3a2a*cos(theta3_rad)*cos(theta4_rad)+x3a2a*sin(theta3_rad)*sin(theta4_rad));
}

//zweite Gleichung
double zweiteGleichung(double theta1_rad, double theta3_rad, double theta4_rad)
{
	return(y1a2*cos(theta1_rad)-y4a3*cos(theta4_rad)-
			x4a3*sin(theta4_rad)-x3a2a*cos(theta4_rad)*sin(theta3_rad)-
			x3a2a*cos(theta3_rad)*sin(theta4_rad)-y3a2a*cos(theta3_rad)*cos(theta4_rad)+y3a2a*sin(theta3_rad)*sin(theta4_rad));
}

//dritte Gleichung für theta2
double gl_theta2(double theta1_rad, double theta2_rad, double theta3_rad, double theta4_rad)
{
	return(cos(theta1_rad)*cos(theta2_rad)-cos(theta3_rad)*cos(theta4_rad)-sin(theta1_rad)*sin(theta2_rad)+sin(theta3_rad)*sin(theta4_rad));
}
//partielle Ableitung f_theta3
double f_theta3()
{
	return (ersteGleichung(theta1_rad, theta3_rad+delta, theta4_rad)-ersteGleichung(theta1_rad, theta3_rad, theta4_rad))/delta;
}

//partielle Ableitung f_theta4
double f_theta4()
{
	return((ersteGleichung(theta1_rad, theta3_rad, theta4_rad+delta)-ersteGleichung(theta1_rad, theta3_rad, theta4_rad))/delta);
}
//partielle Ableitung g_theta3
double g_theta3()
{
	return((zweiteGleichung(theta1_rad, theta3_rad+delta, theta4_rad)-zweiteGleichung(theta1_rad, theta3_rad, theta4_rad))/delta);
}

//partielle Ableitung g_theta4
double g_theta4()
{
	return((zweiteGleichung(theta1_rad, theta3_rad, theta4_rad+delta)-zweiteGleichung(theta1_rad, theta3_rad, theta4_rad))/delta);
}
//korrekturvektor ksi_theta3
double ksi_theta3(double g, double f, double g_theta3, double g_theta4, double f_theta3, double f_theta4)
{
	return( ((g*f_theta4)-(f*g_theta4))/((f_theta3*g_theta3)-(f_theta4*g_theta3)) );
}

//korrekturvektor eta_theta4
double eta_theta4(double g, double f, double g_theta3, double g_theta4, double f_theta3, double f_theta4)
{
	return( ((f*g_theta3)-(g*f_theta3))/((f_theta3*g_theta4)-(f_theta4*g_theta3)) );
}
//iteration für theta3 und theta4
void iteration_Theta3_4()
{

	//Startwert für die Iterationen
	double theta3_alt = 0;
	double theta3_neu = 0.01;
	double theta3_temp = 0;

	double theta4_alt = 0;
	double theta4_neu = 0.01;
	double theta4_temp = 0;

	int counter = 0;

	while(((fabs(theta3_alt - theta3_neu )) > genauigkeit) && (fabs(theta4_alt - theta4_neu ) > genauigkeit) && (counter < iterationsAnzahl))
	{
		theta3_temp = theta3_alt + ksi_theta3(zweiteGleichung(theta1_rad,theta3_alt,theta4_alt), ersteGleichung(theta1_rad,theta3_alt,theta4_alt), g_theta3(theta1_rad,theta3_alt,theta4_alt), g_theta4(theta1_rad,theta3_alt,theta4_alt), f_theta3(theta1_rad,theta3_alt,theta4_alt), f_theta4(theta1_rad,theta3_alt,theta4_alt));
		theta3_alt = theta3_neu;
		theta3_neu = theta3_temp;
	
		theta4_temp = theta4_alt + eta_theta4(zweiteGleichung(theta1_rad,theta3_alt,theta4_alt), ersteGleichung(theta1_rad,theta3_alt,theta4_alt), g_theta3(theta1_rad,theta3_alt,theta4_alt), g_theta4(theta1_rad,theta3_alt,theta4_alt), f_theta3(theta1_rad,theta3_alt,theta4_alt), f_theta4(theta1_rad,theta3_alt,theta4_alt));
		theta4_alt = theta4_neu;
		theta4_neu = theta4_temp;

		counter++;
	}
	theta3_rad = theta3_neu;
	theta4_rad = theta4_neu;
	
}

//iteration für theta2
void iteration_Theta2()
{
        double theta2_alt = 0;
        double theta2_neu = - 0.0001;
        double theta2_temp = 0;

        int counter = 0;

        while((((fabs(theta2_alt - theta2_neu )) > genauigkeit) && (fabs(gl_theta2(theta1_rad,theta2_neu,theta3_rad,theta4_rad)) > genauigkeit)) && (counter < iterationsAnzahl))
        {
        theta2_temp = theta2_neu - ((theta2_neu - theta2_alt) / (gl_theta2(theta1_rad,theta2_neu,theta3_rad, theta4_rad) - gl_theta2(theta1_rad,theta2_alt,theta3_rad,theta4_rad))) * gl_theta2(theta1_rad,theta2_neu, theta3_rad,theta4_rad);
        theta2_alt = theta2_neu;
        theta2_neu = theta2_temp;
	
	printf("Theta2 ist nun: %lf\n", theta2_neu);
        counter++;
	
        }
	printf("z: %d \n", counter);
        theta2_rad = theta2_neu;

}

void init(void)
{
  glClearColor (0.0, 0.0, 0.0, 0.0);
  glShadeModel (GL_FLAT);
}


void display(void)
{
   glClear (GL_COLOR_BUFFER_BIT);

	glPushMatrix();
	
	glLineWidth(2.0);
	 glColor3f(1.0, 1.0, 1.0);
	 glBegin(GL_LINES);
		  glVertex2f(-180.0, 0.0);
		  glVertex2f(180.0, 0.0);
	  glEnd();

	  //koerper 1
	 glColor3f(0.0, 0.0, 1.0);
	 glTranslatef(-100, 0.0, 0.0);
	 glRotatef(RadToDeg(theta1_rad), 0.0, 0.0, 1.0);	
	 glBegin(GL_LINES);
		  glVertex2f(0.0, 0.0);
		  glVertex2f(0.0, 60);
	 glEnd();
	 glPopMatrix();


	  //koerper 2
	 glPushMatrix();
	  glColor3f(0.0, 1.0, 0.0);
	  glBegin(GL_LINES);
		   glVertex2f(x_koerper1, y_koerper1);
		   glVertex2f(x_koerper3, y_koerper3);
	glEnd();
	glPopMatrix();


	  //koerper 3
 	glPushMatrix();
	  glColor3f(1.0, 0.0, 0.0);
	  glTranslatef(100, 0.0, 0.0);
	  glRotatef(RadToDeg(theta4_rad), 0.0, 0.0, 1.0);
	  glBegin(GL_LINES);
		   glVertex2f(0.0, 0.0);
		   glVertex2f(40.0, 160.0);
	  glEnd();
	glPopMatrix();

	glutSwapBuffers();
      glFlush();
}

void specialKey(int key, int x, int y)
{
    switch (key) {
	case GLUT_KEY_RIGHT:	
		theta1_deg = (theta1_deg - 5) % 360;
        break;
	case GLUT_KEY_LEFT:
		//Modulo nur damit man keine winkel > 360 bekommen kann
		theta1_deg = (theta1_deg + 5) % 360;
        break;
     }
	//Nun Theta1 ausrechnen
	theta1_rad = DegToRad(theta1_deg);
	//jetzt Tehta45 ausrechnen lassen
	iteration_Theta3_4();

	//iteration für theta2
	iteration_Theta2();
	//2. Koerper malen
	berechneKoordinatenFuerKoerper2();

	printf("Theta1: %lf\n Theta2: %lf\n Theta3: %lf\n Theta4: %lf\n",RadToDeg(theta1_rad), RadToDeg(theta2_rad), RadToDeg(theta3_rad), RadToDeg(theta4_rad));
     	//Neu zeichnen lassen
     	glutPostRedisplay();
     	                                
}

void reshape (int w, int h)
{
   glViewport (0, 0, (GLsizei) w, (GLsizei) h);
   glMatrixMode (GL_PROJECTION);
   glLoadIdentity ();
   glOrtho(-200.0,200.0,-200.0,200.0,-200.0,200.0);
   glMatrixMode(GL_MODELVIEW);
   glLoadIdentity();
}
	
int main(int argc, char** argv)
{
   glutInit(&argc, argv);
   glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB);
   glutInitWindowSize (700, 700); 
   glutInitWindowPosition (100, 100);
   glutCreateWindow (argv[0]);
   init ();
   glutDisplayFunc(display); 
      glutReshapeFunc(reshape);
   glutSpecialFunc(specialKey);
   iteration_Theta3_4();
   iteration_Theta2();
   glutMainLoop();
   return 0;
}

double DegToRad(int deg)
{
	printf("Deg: %d zu Rad: %lf\n", deg, (deg * 0.017453293));
	return (deg * 0.017453293);
}

//umwandeln von rad zu grad
double RadToDeg(double rad)
{
	double deg = 0;
	deg = 180.0 / M_PI * rad;
	return deg;
}
